//ITMD536 Software Testing and Maintenance.
//Final project <group assignment>
//Author: Wanglei ID:A20479012
#include <iostream>
#include <string>
#include <vector>
#include <algorithm>
#include <map>
#include "Face.hpp"

//for face landmark.
extern ncnn::Net lmNet;

static int extractkeypoint(cv::Mat &imgsrc, FaceLmkType* lmdBox){
	int cycI;
	//int img_h = LM_ALIGNED_H;
	//int img_w = LM_ALIGNED_W;
	
	const float mean_vals[3] = {127.5f, 127.5f, 127.5f};
	const float norm_vals[3] = {1.0/127.5,1.0/127.5,1.0/127.5};

	cv::Rect face;
	face.x = (int)lmdBox->box.x0;
	face.y = (int)lmdBox->box.y0;
	face.width = (int)lmdBox->box.x1-lmdBox->box.x0;
	face.height = (int)lmdBox->box.y1-lmdBox->box.y0;

	cv::Mat img_face = imgsrc(face).clone();
	ncnn::Mat out;
	ncnn::Mat in = ncnn::Mat::from_pixels_resize(img_face.data, ncnn::Mat::PIXEL_BGR, 
		img_face.cols, img_face.rows, FACE_ALIGNED_W, FACE_ALIGNED_H);
	in.substract_mean_normalize(mean_vals, norm_vals);

	//create the ncnn extractor.
	ncnn::Extractor ex = lmNet.create_extractor();

	//input to ncnn engine.
	ex.input("data", in);
	//printf("landmark: in w=%d h=%d c=%d dims=%d\n", in.w, in.h, in.c, in.dims);
	//ncnn engine extract ...
	ex.extract("bn6_3",out);
	//printf("landmark: out w=%d h=%d c=%d dims=%d\n", out.w, out.h, out.c, out.dims);
	//copy the out data for calc landmark position.
	for (cycI = 0; cycI<LM_FEATURE_LEN; cycI++) 
	{
		lmdBox->lx[cycI] = abs(out[2 * cycI] * img_face.cols) + face.x;
		lmdBox->ly[cycI] = abs(out[2 * cycI + 1] * img_face.rows)+face.y;
	}
	
	return faceNormal; 
}

int Face::faceLandmark(cv::Mat& frame, FaceLmkType *lmdBox){

	int ret;
	//cv::Mat process_mat;	
#if 0
	FaceBox faceBox;
	
	//input face rect.
	faceBox.x0 = lmdBox->box.x0;
	faceBox.y0 = lmdBox->box.y0;
	faceBox.x1 = lmdBox->box.x1;
	faceBox.y1 = lmdBox->box.y1;
	for (cycI=0; cycI<5; cycI++) 
	{//input face landmark.
		faceBox.landmark.x[cycI] = lmdBox->box.landmark.x[cycI];
		faceBox.landmark.y[cycI] = lmdBox->box.landmark.y[cycI];
	}
#endif

	//face align.
	//process_mat = rAlignFace(frame, faceBox);
	ret = extractkeypoint(frame, lmdBox);
	if (ret != faceNormal)
		return faceLandmarkError;
	return faceNormal;
}
